% clc;
% clear;
% 
% 
% %这里的t-ed指的是一半的阶段的值
% t_jerk = 0.25;
% t_ed = 1;
% t_half_idle = 0.5;
% pos_st = 0;
% jerk_value = 4*pi/3;
% 
% 
% period = 0.01;
% steps = (t_ed+t_half_idle)*2/period+1;
% vel_s_traj_value = vel_s_traj_info(jerk_value,t_jerk,t_ed,t_half_idle,period,pos_st);
% 
% 
% %  @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
% %integral(@(time) vel_s_traj_vel(jerk_value,t_jerk,t_ed,t_half_idle,time),0,t)+pos_st
% % pos_final = @(jerk_value)(integral(@(time) vel_s_traj_vel(jerk_value,t_jerk,t_ed,t_half_idle,time),0,2*(t_ed+t_half_idle))+pos_st-pos_ed);
% % jerk_value = fsolve(pos_final,0);
% 
% %完了，这个急动度好像不好整
% 
% t = 0:period:(t_ed+t_half_idle)*2;
% subplot(3,1,1);
% plot(t,vel_s_traj_value(:,1));
% title("position");
% subplot(3,1,2);
% plot(t,vel_s_traj_value(:,2));
% title("velocity");
% subplot(3,1,3);
% plot(t,vel_s_traj_value(:,3));
% title("acc");



clc;
clear;


%这里的t-ed指的是一半的阶段的值
t_jerk = 1;
t_ed = 2.5;
t_half_idle = 1;
pos_st = 0;
jerk_value = pi/20;


period = 0.01;
steps = (t_ed+t_half_idle)*2/period+1;
vel_s_traj_value = vel_s_traj_info(jerk_value,t_jerk,t_ed,t_half_idle,period,pos_st);


%  @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
%integral(@(time) vel_s_traj_vel(jerk_value,t_jerk,t_ed,t_half_idle,time),0,t)+pos_st
% pos_final = @(jerk_value)(integral(@(time) vel_s_traj_vel(jerk_value,t_jerk,t_ed,t_half_idle,time),0,2*(t_ed+t_half_idle))+pos_st-pos_ed);
% jerk_value = fsolve(pos_final,0);

%完了，这个急动度好像不好整

t = 0:period:(t_ed+t_half_idle)*2;
subplot(3,1,1);
plot(t,vel_s_traj_value(:,1));
title("position");
subplot(3,1,2);
plot(t,vel_s_traj_value(:,2));
title("velocity");
subplot(3,1,3);
plot(t,vel_s_traj_value(:,3));
title("acc");